% xyz_MRI_fit6.m - returns total distance between all elp points and the
% nearest surface points.  to minimize, call with x = fminsearch(fun,x0)
%
% uses 6 affine parameters:
%
% x0 = x-trans  y-trans  z-trans
%      x-angle  y-angle  z-angle
%
% 1) apply transform parameters "x0" to elp.sensor_xyz points
% 2) find nearest points to elp points in scalp vertex array "vert1"
% 3) sum distances between elp and scalp points
%
% The original code, using Matlabs transform routines, resulted in
% no (or very little) translation.  Elena's code produces significant
% translation.  We don't know why...
%
% using Steven Michael's kdtree routine from:
% http://www.mathworks.com/matlabcentral/fileexchange/
%  7030-kd-tree-nearest-neighbor-and-range-search
%
% !!! pass flags.params to choose scale, rigid body or both (shear?)

function dist = xyz_MRI_fit6(x0)

% no point copying these 10^ times
%
global xyz_elp xyz_elp_TR TreeRoot TreeRoot2 ClosestPts tai flags
global i_ter trackx0 track_dist xyz_scalp

trackx0(i_ter,:)=x0;
i_ter=i_ter+1;

% 1) apply transform parameters "x0" to elp.sensor_xyz points
% Use Elena's matrices
%
% Translation: 
%                | 1 0 0 dx |
% T(dx,dy,dz) =  | 0 1 0 dy |
%                | 0 0 1 dz |
%                | 0 0 0 1  |

TR=[1 0 0 x0(1); 0 1 0 x0(2); 0 0 1 x0(3); 0 0 0 1]; 

% Scaling: 
%                | sx  0  0  0 |
% S(sx,sy,sz) =  |  0 sy  0  0 |
%                |  0  0 sz  0 |
%                |  0  0  0  1 |

% SC=[x0(7) 0 0 0 ; 0 x0(8) 0 0 ; 0 0 x0(9) 0; 0 0 0 1];

% Rotation: 
%         | 1      0        0   0 |
% Rx(A) = | 0  cos A   -sin A   0 |
%         | 0  sin A    cos A   0 |
%         | 0      0        0   1 |

Rx=[1 0 0 0 ; 0 cos(x0(4)) -sin(x0(4)) 0; 0 sin(x0(4)) cos(x0(4)) 0; 0 0 0 1];

%         | cos A   0   sin A   0 |
% Ry(A) = |     0   1       0   0 |
%         | -sin A  0   cos A   0 |
%         |     0   0       0   1 |

Ry=[cos(x0(5)) 0 sin(x0(5)) 0; 0 1 0 0 ; -sin(x0(5)) 0 cos(x0(5)) 0; 0 0 0 1]; 

%         | cos A  -sin A   0   0 |
% Rz(A) = | sin A   cos A   0   0 |
%         |     0       0   1   0 |
%         |     0       0   0   1 |
        
Rz=[cos(x0(6)) -sin(x0(6)) 0 0 ; sin(x0(6)) cos(x0(6)) 0 0 ; 0 0 1 0; 0 0 0 1];

% define & apply combined matrix (N.B. ORDER IS CRITICAL!!!
%
% R = makeresampler('cubic','bound'); % might want to resample scalp ?

AffMat=TR*Rz*Ry*Rx;

% Do our own (Elena's) transform
%
xyzF=[xyz_elp'; repmat(1, [1 size(xyz_elp, 1)])];
TRFxyz=(AffMat*xyzF)';
xyz_elp_TR=TRFxyz(:,1:3);

% plot each fit...
%
% scatter3(1.02*xyz_elp_TR(:,1)+tai.origin(1,1),...
%              1.02*xyz_elp_TR(:,2)+tai.origin(1,2),...
%              1.02*xyz_elp_TR(:,3)+tai.origin(1,3),20,'g');
% pause(1)

% 2) find nearest surface point to each "elp" point
%
if flags.kdtree_type==1
    % Steven Michael's kdtree routine:
%     [Pts_idx,ClosestPts] = kdtree_closestpoint(TreeRoot,xyz_elp_TR);
    Pts_idx = knnsearch(TreeRoot2,xyz_elp_TR);
    ClosestPts=xyz_scalp(Pts_idx,:);
elseif flags.kdtree_type==2;
    % pramod vemulapalli's routine:
    for i=1:length(xyz_elp_TR) % can't handle elp matrix :(
%         size(TreeRoot)
%         disp('pausing');pause
        [Pts_idx,ClosestPts(i,:),node_number] =...
            kd_closestpointfast(TreeRoot,xyz_elp_TR(i,:));
%         disp('size(TreeRoot')
%         size(TreeRoot)
    end
else
    disp('Please set flags.kdtree_type to 1 or 2')
    return
end

% 3) compute the distance for each point (don't bother w/sqrt!)
%
pts_dist=((xyz_elp_TR(:,1)-ClosestPts(:,1)).^2+...
          (xyz_elp_TR(:,2)-ClosestPts(:,2)).^2+...
          (xyz_elp_TR(:,3)-ClosestPts(:,3)).^2);
% and sum them
dist=sum(sqrt(pts_dist))/size(xyz_elp,1);

track_dist(i_ter)=dist;

